
import pygame
import serial
import time
import string
from pygame.joystick import Joystick

#pygame init
pygame.init()
pygame.joystick.init()
try:
	j = Joystick(0) # create a joystick instance
	j.init() # init instance
	print 'Enabled joystick: ' + j.get_name()
except pygame.error:
	print 'no joystick found.'

#serial init
s = serial.Serial(port='COM8',baudrate=57600, timeout=1)

try:
        row = 0
        yaw = 0
        pit = 0
        z = 0
        eStop = 0
        for i in range(1,50):
            s.write('i')
            print "i"
        while 1:
                e = pygame.event.poll() 
                #print 'event : ' + str(e.type)
                if e.type == pygame.JOYAXISMOTION: # 7
                        #Capture all axis used and emergency buttons
                        z, yaw ,pit, row  = j.get_axis(1) , j.get_axis(2), j.get_axis(3), j.get_axis(4)
                        eStop = j.get_button(0) | j.get_button(1) | j.get_button(2) | j.get_button(3) 
		#Process Joystick input
                        z = str(int(abs(z)*255))
                        pit = str(int(((pit+1)/2)*1000))
                        row = str(int(((row+1)/2)*1000))
                        yaw = str(int(((yaw+1)/2)*1000))
                #Package Joystick input:
                        z = "z" + str(len(z)) + z + "!"
                        pit = "p" + str(len(pit)) + pit + "!"
                        row = "r" + str(len(row)) + row + "!"
                        yaw = "y" + str(len(yaw)) + yaw + "!"
                #Write Joystick:
                if eStop == 1:
                        s.write('e')
                elif eStop == 0:
                        s.write(z)
                        s.write(pit)
                        s.write(row)
                        s.write(yaw)
                #Check for message:
                if s.readable() == True:
                        print z
                        print s.readline()
                            
                #time.sleep(0.01)
                #print("Joy:" + str(pit)+ "\t" + str(row)+ "\t" + str(yaw)  )
except KeyboardInterrupt:
        s.close()
        
        
